package com.shanlitech.bluetooth;

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.util.Log;

import com.kircherelectronics.fsensor.filter.gyroscope.fusion.kalman.OrientationFusedKalman;
import com.kircherelectronics.fsensor.linearacceleration.LinearAcceleration;
import com.kircherelectronics.fsensor.linearacceleration.LinearAccelerationFusion;
import com.kircherelectronics.fsensor.util.gravity.GravityUtil;
import com.kircherelectronics.fsensor.util.rotation.RotationUtil;
import com.shanlitech.lbs.types.AccelerationRecord;
import com.shanlitech.lbs.utils.TimedValuesLogger;

import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;

public class AccelerationSensor {
    private static String TAG = "SLBluetooth";
    //private static float FIX_X = -0.00123f;
    //private static float FIX_Y = -0.00618f;
    //private static float FIX_Z = 0.4243f;

    private final Context mContext;
    private final List<AccelerationRecord> mRecords = new LinkedList<>();
    private SensorManager mSensorManager;
    private OrientationFusedKalman mOrientation;
//    private TimedValuesLogger mLogger = new TimedValuesLogger("acceleration.dat");

    private final float[] raw_acceleration = new float[3];
    private final float[] acceleration = new float[3];
    private final float[] magnetic = new float[3];
    private final float[] gyroscope = new float[3];
    private boolean has_magnetic = false;
    private boolean has_gyroscope = false;
    final float[] earth_acceleration = new float[3];

    private final SensorEventListener mObserver = new SensorEventListener() {
        @Override
        public void onSensorChanged(SensorEvent event) {
            if( event.sensor.getType() == Sensor.TYPE_ACCELEROMETER ) {
                System.arraycopy(event.values,0,raw_acceleration,0,event.values.length);
                if( ! mOrientation.isBaseOrientationSet() ) {
                    if (has_magnetic && has_gyroscope) {
                        mOrientation.setBaseOrientation(RotationUtil.getOrientationVectorFromAccelerationMagnetic(raw_acceleration, magnetic));
                    }
                } else {
                    final float[] orientation = mOrientation.calculateFusedOrientation(gyroscope,event.timestamp,raw_acceleration,magnetic);
                    if( ! isFloatArrayOk(orientation,"orientation") ) {
                        return;
                    }

                    float[] gravity = GravityUtil.getGravityFromOrientation(orientation);
                    if( ! isFloatArrayOk(gravity,"gravity") ) {
                        return;
                    }
                    acceleration[0] = raw_acceleration[0] - gravity[0];
                    acceleration[1] = raw_acceleration[1] - gravity[1];
                    acceleration[2] = raw_acceleration[2] - gravity[2];

                    final float[] rotationMatrix = new float[9];
                    SensorManager.getRotationMatrix(rotationMatrix,null,gravity,magnetic);
                    if( ! isFloatArrayOk(rotationMatrix,"rotationMatrix") ) {
                        return;
                    }
                    earth_acceleration[0] = acceleration[0] * rotationMatrix[0] + acceleration[1] * rotationMatrix[1] + acceleration[2] * rotationMatrix[2];
                    earth_acceleration[1] = acceleration[0] * rotationMatrix[3] + acceleration[1] * rotationMatrix[4] + acceleration[2] * rotationMatrix[5];
                    earth_acceleration[2] = acceleration[0] * rotationMatrix[6] + acceleration[1] * rotationMatrix[7] + acceleration[2] * rotationMatrix[8];
                    //mLogger.add(earth_acceleration);

                    final long now = (long)(event.timestamp / 1000000.0);
                    mRecords.add(new AccelerationRecord(now,earth_acceleration[0],earth_acceleration[1],earth_acceleration[2]));
                }
            } else if( event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD ) {
                System.arraycopy(event.values,0,magnetic,0,event.values.length);
                has_magnetic = true;
            } else if( event.sensor.getType() == Sensor.TYPE_GYROSCOPE ) {
                System.arraycopy(event.values,0,gyroscope,0,event.values.length);
                has_gyroscope = true;
            }
        }

        @Override
        public void onAccuracyChanged(Sensor sensor, int i) {
        }
    };

    public AccelerationSensor(Context context) {
        mContext = context;
        mSensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE);
        mOrientation = new OrientationFusedKalman();
    }

    public void start() {
        mRecords.clear();
        mOrientation.reset();
        mOrientation.startFusion();
        mSensorManager.registerListener(mObserver,mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER),SensorManager.SENSOR_DELAY_GAME);
        mSensorManager.registerListener(mObserver,mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE),SensorManager.SENSOR_DELAY_GAME);
        mSensorManager.registerListener(mObserver,mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD),SensorManager.SENSOR_DELAY_GAME);
    }

    public void stop() {
        mSensorManager.unregisterListener(mObserver);
        mOrientation.stopFusion();
        //mLogger.write();
        mRecords.clear();
    }

    public List<AccelerationRecord> getReset() {
        final List<AccelerationRecord> records = new ArrayList<>(mRecords);
        mRecords.clear();
        return records;
    }

    private static boolean isFloatArrayOk(final float[] values,final String name) {
        if( values == null ) {
//            Log.e(TAG,name + " null");
            return false;
        }
        boolean rc = true;
        for(float v : values) {
            if( Float.isNaN(v) || Float.isInfinite(v) ) {
                rc = false;
                break;
            }
        }
        /*
        if( ! rc ) {
            final StringBuilder sb = new StringBuilder("Bad ");
            sb.append(name).append(":");
            for(float v : values) {
                sb.append(" ").append(v);
            }
            Log.e(TAG,sb.toString());
        }
         */
        return rc;
    }
}
